#include <DeformingPolygon.h>
#include <map>
#include <szMiscOperations.h>
#include <szMexUtility.h>
#include <IntersectionConvexPolygons.h>
#include <mex.h> //just for printf

/*
A unit vector from o to x in 2D
*/
CParticleF 
NormalizedDirection(CParticleF& x, CParticleF& o)
{
	float dx = x.m_X - o.m_X;
	float dy = x.m_Y - o.m_Y;
	float len = sqrt(dx*dx + dy*dy);
	if(len > 1.0e-5)
	{
		return CParticleF(dx/len, dy/len);
	}
	else
	{
		return CParticleF(0.0f, 0.0f);
	}
}

/*
A unit vector from o to x in 3D
*/
CParticleF 
NormalizedDirection3D(CParticleF& x, CParticleF& o)
{
	float dx = x.m_X - o.m_X;
	float dy = x.m_Y - o.m_Y;
	float dz = x.m_Z - o.m_Z;
	float len = sqrt(dx*dx + dy*dy + dz*dz);
	if(len > 1.0e-5)
	{
		return CParticleF(dx/len, dy/len, dz/len);
	}
	else
	{
		return CParticleF(0.0f, 0.0f, 0.0f);
	}
}

/*
return a unit vector that bisects the angle formed by three points: a-o-b.
*/
CParticleF
bisector(CParticleF& o, CParticleF& a, CParticleF& b)
{
	CParticleF x = NormalizedDirection(a, o);
	CParticleF y = NormalizedDirection(b, o);
	CParticleF z((x.m_X+y.m_X)/2, (x.m_Y+y.m_Y)/2);
	float vx = z.m_X;
	float vy = z.m_Y;
	float len0 = sqrt(vx*vx + vy*vy);
	if(len0 <= 1.0e-5) //this is a colinear point. 
	{
		float ang = GetVisualDirection(b.m_X, b.m_Y, a.m_X, a.m_Y) + PI/2.0;
		vx = cos(ang);
		vy = sin(ang);
	}
	else
	{
		vx = vx / len0;
		vy = vy / len0;
	}
	return CParticleF(vx, vy);
}

float
intersectPlaneAndLine(CParticleF op, CParticleF& z, CParticleF& ol, CParticleF u)
{
	//move the origin to ol.
	op.m_X -= ol.m_X;
	op.m_Y -= ol.m_Y;
	op.m_Z -= ol.m_Z;
	float t = (z.m_X * op.m_X + z.m_Y * op.m_Y + z.m_Z * op.m_Z) / (z.m_X * u.m_X + z.m_Y * u.m_Y + z.m_Z * u.m_Z);
	return t;
}

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, float precision = 1.0e-3)
{
	float d = Distance2Line(a, c, b);
	return d < precision;
}

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, CParticleF& d, float precision = 1.0e-3)
{
	float d1 = Distance2Line(a, c, b);
	float d2 = Distance2Line(a, c, d);
	return d1 < precision && d2 < precision;
}

CParticleF
crossProduct(CParticleF& a, CParticleF& b)
{
	return CParticleF(a.m_Y*b.m_Z-a.m_Z*b.m_Y, a.m_Z*b.m_X-a.m_X*b.m_Z, a.m_X*b.m_Y-a.m_Y*b.m_X);
}

/*
Given three points in 3D, define a unit vector perpendicular to the plane going through the three points.
The vector is always upward (i.e. its Z-coordinate is always positive). But this should not really matter...
*/
CParticleF
perp2Plane(CParticleF& a, CParticleF& b, CParticleF& c)
{
	CParticleF u = NormalizedDirection3D(b, a);
	CParticleF v = NormalizedDirection3D(c, a);
	CParticleF z = crossProduct(u, v);
	if(z.m_Z < 0)
	{
		z.m_X = -z.m_X;
		z.m_Y = -z.m_Y;
		z.m_Z = -z.m_Z;
	}
	return z;
}

bool
calculateVelocity(CParticleF& a, CParticleF& o, CParticleF& b, float& vx, float& vy)
{
	CParticleF bs = bisector(o, a, b);
	float ang = GetVisualAngle2(a.m_X, a.m_Y, b.m_X, b.m_Y, o.m_X, o.m_Y);
	float cs = cos((PI - Abs(ang)) / 2.0);
	float len = 1.0f / cs;
	bs.m_X *= len;
	bs.m_Y *= len;
	if(ang<0)
	{
		bs.m_X = -bs.m_X;
		bs.m_Y = -bs.m_Y;
	}
	vx = bs.m_X;
	vy = bs.m_Y;
	return true;
}

bool 
MovingParticle::setVelocity()
{
	if(next && prev)
	{
		if(next == prev)
		{
			NormalizedDirection(next->p, p);
		}
		else
		{
			calculateVelocity(prev->p, p, next->p, v[0], v[1]);
		}
		return true;
	}
	else
	{
		v[0] = 0; v[1] = 0;
		return false;
	}
}


DeformingPolygon::DeformingPolygon(vector<CParticleF>& pnts)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	float eps = 1.0e-5;
	for(int i=0; i<pnts.size(); ++i)
	{
		MovingParticle* p = factory->makeParticle(pnts[i]);
		particles.push_back(p);
	}
	for(int i=0; i<particles.size(); ++i)
	{
		//particles[i]->p0 = particles[i]->p;
		int j = (i+1) % particles.size();
		particles[i]->next = particles[j];
		int k = (i-1+particles.size()) % particles.size();
		particles[i]->prev = particles[k];
		particles[i]->setVelocity();
	}
}

pair<float,MovingParticle*>
DeformingPolygon::nextEdgeEvent()
{
	float t = std::numeric_limits<float>::infinity();
	pair<float,MovingParticle*> res(t, (MovingParticle*)NULL);
	for(int i=0; i<particles.size(); ++i)
	{
		if(particles[i]->id+1==402)
			i+=0;
		CParticleF p2 = particles[i]->move(1);
		CParticleF q2 = particles[i]->next->move(1);
		pair<float,float> param = _IntersectConvexPolygon::intersect(particles[i]->p, p2, particles[i]->next->p, q2);
		float t0 = Max(param.first, param.second);
		if(param.first>0 && param.second >0 && t0 < t)
		{
			t = t0;
			res.first = t;
			res.second = particles[i];
		}
	}
	return res;
}

pair<float,MovingParticle*>
DeformingPolygon::nextSplitEvent()
{
	float t = std::numeric_limits<float>::infinity();
	pair<float,MovingParticle*> res(t, (MovingParticle*)NULL);
	for(int i=0; i<particles.size(); ++i)
	{
		CParticleF bs = bisector(particles[i]->p, particles[i]->prev->p, particles[i]->next->p);
		if(bs.m_X * particles[i]->v[0] + bs.m_Y * particles[i]->v[1] < 0) //concave
		{
			CParticleF ol = particles[i]->p;
			CParticleF ul(particles[i]->v[0], particles[i]->v[1], 1.0);
			for(int j=0; j<particles.size(); ++j)
			{
				if(particles[i]==particles[j] || particles[i]->next==particles[j] || particles[i]->prev==particles[j]) continue;
				CParticleF op = particles[j]->p;
				CParticleF oq = particles[j]->next->p;
				CParticleF or(op.m_X + particles[j]->v[0], op.m_Y + particles[j]->v[1], 1.0f);
				CParticleF up = perp2Plane(op, oq, or);
				float t0 = intersectPlaneAndLine(op, up, ol, ul);
				if(t0 > 0 && t0<t) 
				{
					//check if the intersection point is within the sweep of the line segment.
					CParticleF loc(ol.m_X+t0*ul.m_X, ol.m_Y+t0*ul.m_Y);//the location of intersection in 2D
					CParticleF v1(particles[j]->v[0], particles[j]->v[1], 0.0f); 
					CParticleF dp1 = NormalizedDirection(loc, op);
					CParticleF cp1 = crossProduct(v1, dp1);
					CParticleF v2(particles[j]->next->v[0], particles[j]->next->v[1], 0.0f); 
					CParticleF dp2 = NormalizedDirection(loc, oq);
					CParticleF cp2 = crossProduct(v2, dp2);
					if(cp1.m_Z * cp2.m_Z < 0 || Abs(cp1.m_Z) < 1.0e-5 || Abs(cp2.m_Z) < 1.0e-5)
					{
						t = t0;
						res.first = t;
						res.second = particles[i];
					}
				}
			}
		}
	}
	return res;
}

DeformingPolygon
DeformingPolygon::split(int begin, int end)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	DeformingPolygon poly(*this);
	vector<MovingParticle*> copy = particles;

	vector<MovingParticle*> particles1;
	particles1.push_back(factory->makeParticle(copy[begin]->p));
	for(int i=(begin+1) % copy.size(); i!=(end+1) % copy.size(); i=(i+1) % copy.size())
	{
		particles1.push_back(copy[i]);
	}
	//for(int i=0; i<particles1.size(); ++i)
	{
		int i0 = particles1.size()-1;
		int i2 = 1;
		particles1[0]->prev = particles1[i0];
		particles1[0]->next = particles1[i2];
		particles1[0]->prev->next = particles1[0];
		particles1[0]->next->prev = particles1[0];
		particles1[0]->setVelocity();
	}

	vector<MovingParticle*> particles2;
	particles2.push_back(factory->makeParticle(copy[begin]->p));
	for(int i=(end+1) % copy.size(); i!=begin; i=(i+1) % copy.size())
	{
		particles2.push_back(copy[i]);
	}
	//for(int i=0; i<particles2.size(); ++i)
	{
		int i0 = particles2.size()-1;
		int i2 = 1;
		particles2[0]->prev = particles2[i0];
		particles2[0]->next = particles2[i2];
		particles2[0]->prev->next = particles2[0];
		particles2[0]->next->prev = particles2[0];
		particles2[0]->setVelocity();
	}

	if(particles1.size() < particles2.size())
	{
		this->particles = particles2;
		poly.particles = particles1;
	}
	else
	{
		this->particles = particles1;
		poly.particles = particles2;
	}
	return poly;
}

bool
DeformingPolygon::sanityCheck(int& error_code, int& error_loc)
{
	for(int i=0; i<particles.size(); ++i)
	{
		int i0 = i==0 ? particles.size()-1: i-1;
		int i2 = i==particles.size()-1 ? 0: i+1;
		if(particles[i]->prev != particles[i0])
		{
			error_code = 1;
			error_loc = i;
			return false;
		}
		else if(particles[i]->next != particles[i2])
		{
			error_code = 2;
			error_loc = i;
			return false;
		}
		for(int j=i+1; j<particles.size(); j++)
		{
			if(particles[i] == particles[j])
			{
				error_code = 3;
				error_loc = i;
				return false;
			}
		}
	}
	return true;
}

vector<DeformingPolygon>
fixPolygon(DeformingPolygon& polygon, float eps)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	//find any duplicates (as the result of edge events)
	while(polygon.particles.size()>1)
	{
		bool bChanged = false;
		for(int i=0; i<polygon.particles.size(); ++i)
		{
			MovingParticle* pi = polygon.particles[i];
			MovingParticle* pj = polygon.particles[i]->next;
			//if(pj == NULL) break; //there must be only one point. This should not happen...

			if(Distance(pi->p, pj->p) < eps)
			{
				if(pi->id+1==402 && pj->id+1==403)
					i+=0;
				//create a new particle and insert it between the colliding particles in the array
				MovingParticle* p = factory->makeParticle(pi->p);
				if(pi->prev && pi->prev != pj) 
				{
					p->prev = pi->prev;
					p->prev->next = p;
				}
				if(pi->next->next && pi->next->next != pi)
				{
					p->next = pi->next->next;
					p->next->prev = p;
				}
				p->setVelocity();
				polygon.particles.insert(polygon.particles.begin()+i, p);
				polygon.particles.erase(find(polygon.particles.begin(), polygon.particles.end(), pj));
				polygon.particles.erase(find(polygon.particles.begin(), polygon.particles.end(), pi));
				bChanged = true;
				break;
			}
		}
		if(bChanged == false) break;
	}
	//find colinear redundant points
	while(polygon.particles.size()>2)
	{
		bool bChanged = false;
		for(int i=0; i<polygon.particles.size(); ++i)
		{
			if(coLinear(polygon.particles[i]->prev->p, polygon.particles[i]->p, polygon.particles[i]->next->p))
			{
				polygon.particles[i]->prev->next = polygon.particles[i]->next;
				polygon.particles[i]->next->prev = polygon.particles[i]->prev;
				//polygon.particles[i]->prev->setVelocity();
				//polygon.particles[i]->next->setVelocity();
				polygon.particles.erase(polygon.particles.begin()+i);
				bChanged = true;
				break;
			}
		}
		if(bChanged == false) break;
	}
	//find intersection (as the result of split events) or overlap (as the result of edge events)
	vector<DeformingPolygon> splitted;
	while(polygon.particles.size()>3)
	{
		bool bChanged = false;
		for(int i=0; i<polygon.particles.size(); ++i)
		{
			int loc = -1;
			for(int j=0; j<polygon.particles.size(); ++j)
			{
				if(polygon.particles[i]->id+1==401 && (polygon.particles[j]->id+1==404 || polygon.particles[j]->next->id+1==404))
					i+=0;
				if(polygon.particles[i]->id+1==404 && polygon.particles[j]->id+1==401)
					i+=0;
				if(polygon.particles[j] == polygon.particles[i] || polygon.particles[j]->next == polygon.particles[i]) continue;
				float d = Distance2LineSegment(polygon.particles[j]->p, polygon.particles[j]->next->p, polygon.particles[i]->p);
				if(d < 3*eps)
				{
					loc = j;
					break;
				}
			}
			if(loc >= 0)
			{
				DeformingPolygon poly2 = polygon.split(i, loc);
				//if(poly2.particles.size() > 2)
				{
					splitted.push_back(poly2);
				}
				bChanged = true;
				break;
			}
		}
		if(bChanged == false) break;
	}
	if(polygon.particles.size()==2)
	{
		//finish this polygon by creating a segment connecting the two particles.
		MovingParticle* p1 = factory->makeParticle(polygon.particles[0]->p);
		p1->p = polygon.particles[1]->p;
		CParticleF v = NormalizedDirection(p1->p, p1->p0);
		p1->v[0] = v.m_X;
		p1->v[1] = v.m_Y;
		polygon.particles.clear();
		polygon.particles.push_back(p1);
	}

	return splitted;
}

void
_trace(Vertex<CParticleF>* u, Vertex<CParticleF>*prev, vector<CParticleF>& points)
{
	points.push_back(u->key);
	u->color = Black;
	vector<Edge<CParticleF>*> edges;
	if(prev == NULL)
	{
		edges = u->aList;
	}
	else //sort edges in clock wise order
	{
		vector<pair<float,Edge<CParticleF>*>> pairs;
		for(int i=0; i<u->aList.size(); ++i)
		{
			Vertex<CParticleF>* v = u->aList[i]->v;
			float ang = GetVisualAngle2(prev->key.m_X, prev->key.m_Y, v->key.m_X, v->key.m_Y, u->key.m_X, u->key.m_Y);
			if(ang <= 0) ang += 2*PI;
			pairs.push_back(pair<float,Edge<CParticleF>*>(ang, u->aList[i]));
		}
		sort(pairs.begin(), pairs.end());
		for(int i=0; i<pairs.size(); ++i)
		{
			u->aList[i] = pairs[i].second;
		}
	}

	for(int i=0; i<u->aList.size(); ++i)
	{
		Vertex<CParticleF>* v = u->aList[i]->v;
		if(v->color == White)
		{
			_trace(v, u, points);
			points.push_back(u->key);
		}
	}
}

vector<CParticleF>
traceTree(vector<Vertex<CParticleF>*>& tree)
{
	for(int i=0; i<tree.size(); ++i)
	{
		tree[i]->Reset();
	}

	vector<CParticleF> points;
	_trace(tree[0], NULL, points);
	//remove a copy of the first one at the end of the trace
	points.erase(points.end()-1);
	return points;
}

vector<CParticleF>
offsetPolygon(vector<CParticleF>& points, float margin, float eps)
{
	vector<CParticleF> poly;
	for(int i=0; i<points.size(); ++i)
	{
		int i0 = (i-1+points.size()) % points.size();
		int i2 = (i+1) % points.size();
		CParticleF o = points[i];
		CParticleF a = points[i0];
		CParticleF b = points[i2];
		if(Distance(a, o) < eps || Distance(b, o) < eps) 
		{
			continue;
		}
		if(Distance(a, b) > eps)
		{
			float vx, vy;
			calculateVelocity(a, o, b, vx, vy);
			CParticleF q(o.m_X + margin * vx, o.m_Y + margin * vy);
			poly.push_back(q);
			//printf("o[%3.3f,%3.3f] -> [%3.3f,%3.3f]\n", o.m_X, o.m_Y, q.m_X, q.m_Y);
		}
		else //this is a leaf node of the tree.
		{
			float ang = GetVisualDirection(o.m_X, o.m_Y, a.m_X,  a.m_Y);
			float angles[] = {ang+PI/4.0, ang-PI/4.0};
			for(int j=0; j<2; ++j)
			{
				CParticleF q(o.m_X+sqrt(2.0) * margin*cos(angles[j]), o.m_Y+sqrt(2.0)*margin*sin(angles[j]));
				//printf("\to[%3.3f,%3.3f] -> [%3.3f,%3.3f]\n", o.m_X, o.m_Y, q.m_X, q.m_Y);
				poly.push_back(q);
			}
		}
	}
	return poly;
}


